/***************************************************************************
 创建者: 华磊
 开始时间: 201９.３.1３
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2018.5.8 类的具体实现代码编写
 ***************************************************************************/
#include "socketfleetadapter.h"
#include <QTcpServer>
#include <QTcpSocket>
#include <QTimer>
#include "domparser.h"
#include "messagelog.h"
#include "fleetservice.h"

#define IF_DEBUG 1
using namespace std;

SocketFleetAdapter::SocketFleetAdapter(QString configFileIn,
                FleetService *fleetServiceIn, QObject *parent) : QObject(parent)
{
    fleetService=fleetServiceIn;
    isInitialOkStatus=false;
    tcpSocket=NULL;
    tcpServer=NULL;
    heartBeat=0;
    diTrigCount=0;

    maxFleetCatchLineForDebug=10;

    userInfo=new UserInfo;

    if(1==initialConfigFile(configFileIn))
    {
        fleetService->setFleetTextCommandMaxLogLine(maxFleetCatchLineForDebug);
        if(true==isUseFleet)
        {
            tcpSocket = new QTcpSocket;
            timer=new QTimer();
            connect( timer, SIGNAL(timeout()), this, SLOT(timeOut_slot()) );
            timer->start( 10 );//10ms处理一条请求。
            if(1==initialSocketServer(serverPort))
            {
                   isInitialOkStatus=true;
            }
            else
            {
                addMsg( ENUM_MSG_ERROR,  "SocketFleetAdapter", "SocketFleetAdapter",  12300,  -1);
            }
        }
        else
        {
            isInitialOkStatus=true;
        }

    }
    else
    {
        addMsg( ENUM_MSG_ERROR,  "SocketFleetAdapter", "SocketFleetAdapter",  12301,  -1);
    }
}

int SocketFleetAdapter::getFleetCommandLog(QQueue<QString> &fleetCommandLogOut)
{
    fleetCommandLogOut=fleetCommandLog;
    return 1;
}

void SocketFleetAdapter::timeOut_slot()
{
//    if( NULL==tcpSocket || tcpSocket->state() != QTcpSocket::ConnectedState)
//    {
//        return ;
//    }

    heartBeat++;
    static int sendTick=0;
    sendTick++;
    if(sendTick*10>sendPeriod)//300ms
    {
        sendAgvBasicInformation();
        if(1==isSendMessageString)
        {
              sendMessageInfo();
        }

        if(1==isSendRobotArmInfo)
        {
              sendRobotArmInfo();
        }

        sendTrigPoseQueue();

        sendTick=0;
    }

    if(3==sendTick)
    {
        monitorDi();
    }

    processClientRequest();
}

void SocketFleetAdapter::newConnectionSlot()
{
    QString old_clientIp;
    if(NULL!=tcpSocket)
    {
        old_clientIp=clientIp;
        tcpSocket->disconnect();
        delete tcpSocket;
        passPort_client="";
        clientIp="";
    }

    tcpSocket = new QTcpSocket;
    tcpSocket = tcpServer->nextPendingConnection();
//    if(!tcpSocket->isValid())
//    {
//        return;
//    }
//    if(tcpClientList.values().count() > this->maxPendingConnections())
//    {
//        return;
//    }
    connect(tcpSocket,SIGNAL(error(QAbstractSocket::SocketError)),\
                this,SLOT(socketError(QAbstractSocket::SocketError)),Qt::DirectConnection);
    clientIp = tcpSocket->peerAddress().toString().remove(QRegExp("::ffff:"));
    qDebug() << "fleet new Client IP:" << clientIp;
    QString note=clientIp;
    note+=" oldIp:";
    note+=old_clientIp;
    addMsg( ENUM_MSG_REMIND,  "SocketFleetAdapter", "SocketFleetAdapter",  12302,  -1,note);


}

int SocketFleetAdapter::executeOneCommandAndReply(QString tmp_frame)
{
    qDebug()<<"executeTexCommand:"<<tmp_frame;
     int tmpKey=fleetService->executeTexCommand(*userInfo,tmp_frame);
    if(1==tmpKey)
    {
        qDebug()<<"sendOkFrame,for command:"<<tmp_frame;
        sendOkFrame();
        return 1;
    }
    else if(1000==tmpKey)
    {
        return 1;
    }
    else
    {
        qDebug()<<"sendAbortFrame,error...for command:"<<tmp_frame;
        sendAbortFrame();
        return -1;
    }
    return 1;
}

int SocketFleetAdapter::sendTrigPoseQueue()
{
    if(fleetService->getScantrigPoseGetFlag())
    {
        QQueue<PointPro> trigPoseOut;
        fleetService->getTrigPosition(trigPoseOut);
        fleetService->resetScanTrigPoseGetFlag();


        //组帧
        QString tmp_frame;
        tmp_frame='\n';
        tmp_frame+="TRIGPOSEINFO";
        tmp_frame+="#"+QString::number(trigPoseOut.size())+";";


        for(int n=0;n<trigPoseOut.size();n++)
        {
            for(int i=0;i<6;i++)
            {
                if(i<trigPoseOut[n].positionValue.size())
                {
                    tmp_frame+=QString::number(trigPoseOut[n].positionValue[i],'f',3)+";";
                }
                else
                {
                    tmp_frame+=QString::number(0,'f',3)+";";
                }

            }
        }

        tmp_frame+='\r';
//        qDebug()<<tmp_frame;
        sendData(tmp_frame.toLatin1());

    }
    return 1;
}

#define	GET_BIT(x, bit)	((x & (1 << bit)) >> bit)	/* 获取第bit位 */

int SocketFleetAdapter::monitorDi()
{
    unsigned short tmpLow=fleetService->getDi_short(0);
    static bool diValue_old=false;
    bool diValue=GET_BIT(tmpLow,diMonitorIndex);
    if(false==diValue_old && true==diValue)
    {
        diTrigCount++;
        qDebug()<<"SocketFleetAdapter::monitorDi trigged diTrigCount"<<diTrigCount
               <<"diMonitorIndex"<<diMonitorIndex;
    }
    diValue_old=diValue;

    return 1;
}


int SocketFleetAdapter::initialSocketServer(int port)
{
     tcpServer= new QTcpServer;
    if(!tcpServer->listen(QHostAddress::Any,port))
    {
        qDebug() << "SocketFleetAdapter Server Listen Port Error:" << tcpServer->errorString();
        return -1;
    }
    connect(tcpServer, SIGNAL(newConnection()),  this, SLOT(newConnectionSlot()));
    return 1;
}

int SocketFleetAdapter::initialConfigFile(QString configFileName)
{
    qDebug() << "SocketFleetAdapter::initialConfigFile ==== 初始化配置文件：" << configFileName;

    DomParser domparser;
    bool ok = domparser.openXml(configFileName, QIODevice::ReadOnly);
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << "SocketFleetAdapter::initialConfigFile ==== 配置文件打开失败：" << configFileName;
        }
        return -1;
    }

    QDomNode domNode = domparser.findSubNode(domparser.getRootDomElement(), "isUseFleet", ok );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << "SocketFleetAdapter::initialConfigFile failed" << configFileName;
        }
            return -1;
    }
    domparser.readXml(domNode, isUseFleet);

    domNode = domparser.findSubNode(domparser.getRootDomElement(), "serverPort", ok );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << "SocketFleetAdapter::initialConfigFile failed" << configFileName;
        }
            return -1;
    }
    domparser.readXml(domNode, serverPort);

    domNode = domparser.findSubNode(domparser.getRootDomElement(), "passPort_authority", ok );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << "SocketFleetAdapter::initialConfigFile failed" << configFileName;
        }
            return -1;
    }
    domparser.readXml(domNode, passPort_authority);

    domNode = domparser.findSubNode(domparser.getRootDomElement(), "maxFleetCatchLineForDebug", ok );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << "SocketFleetAdapter::initialConfigFile failed" << configFileName;
        }
            return -1;
    }
    domparser.readXml(domNode, maxFleetCatchLineForDebug);

    domNode = domparser.findSubNode(domparser.getRootDomElement(), "isSendMessageString", ok );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << "SocketFleetAdapter::initialConfigFile failed" << configFileName;
        }
            return -1;
    }
    domparser.readXml(domNode, isSendMessageString);


    domNode = domparser.findSubNode(domparser.getRootDomElement(), "isSendRobotArmInfo", ok );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << "SocketFleetAdapter::initialConfigFile failed isSendRobotArmInfo" << configFileName;
        }
            return -1;
    }
    domparser.readXml(domNode, isSendRobotArmInfo);

    domNode = domparser.findSubNode(domparser.getRootDomElement(), "diMonitorIndex", ok );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << "SocketFleetAdapter::initialConfigFile failed diMonitorIndex" << configFileName;
        }
            return -1;
    }
    domparser.readXml(domNode, diMonitorIndex);

    domNode = domparser.findSubNode(domparser.getRootDomElement(), "sendPeriod", ok );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << "SocketFleetAdapter::initialConfigFile failed sendPeriod" << configFileName;
        }
            return -1;
    }
    domparser.readXml(domNode, sendPeriod);



    domparser.closeXml();
    return 1;
}

int SocketFleetAdapter::sendAgvBasicInformation()
{
    BatteryInformation batteryInfoOut;
    fleetService->getBatteryInfo(*userInfo,batteryInfoOut);
    VehiclePosition positionOut;
    fleetService->getCurrentAgvPosition(*userInfo,positionOut);
    double lineSpeed, rotateSpeed;
    fleetService->getCurrentAgvSpeed(*userInfo,lineSpeed, rotateSpeed);
    E_AGV_STATE stateOut;
    fleetService->getCurrentAgvState(*userInfo,stateOut);
    bool isLoaded;
    fleetService->getLoadedState(*userInfo,isLoaded);
    //
    int errorCode,warnCode,noticeCode;
    fleetService->getMessageInfoCode(errorCode,warnCode,noticeCode);
    double lineDistance,angleDistance;
    fleetService->getTargetPositionDistanceLeft(lineDistance,angleDistance);
    PathMovingInfo pathInfoOut;
    fleetService->getPathMovingInfo(pathInfoOut);
    std::string mapName;
    fleetService->getCurrentMapName(mapName);
    int version=fleetService->getMapVersion();
    unsigned int tmpLow,tmpHigh;
    tmpLow=fleetService->getDi_short(0);
    tmpHigh=fleetService->getDi_short(1);
    long long tmpDi=(tmpHigh<<16)|tmpLow;
//    qDebug()<<"tmpLow"<<tmpLow<<"tmpHigh"<<tmpHigh<<"tmpDi"<<tmpDi;
    tmpLow=fleetService->getDo_short(0);
    tmpHigh=fleetService->getDo_short(1);
    long long tmpDo=(tmpHigh<<16)|tmpLow;
//    qDebug()<<"tmpLow"<<tmpLow<<"tmpHigh"<<tmpHigh<<"tmpDo"<<tmpDo;
    E_LIFT_STATUS liftStatus;
    fleetService->getLiftMechanismStatus(liftStatus);
    E_ROLLER_STATUS rollerStatus;
    fleetService->getRollerMechanismStatus(rollerStatus);
    int tmpFloorId=fleetService->getCurrentFloorId();
    EN_CONTROL_TYPE agvMode=fleetService->getRunModel();
    int tmpBorderId;
    fleetService->getCurrentVehicleBorderId(*userInfo,tmpBorderId);
    E_BARRIER_RUN_MODE barriermodeOut;
    fleetService->getBarrierRunMode(barriermodeOut);


    //组帧
    QString tmp_frame;
    tmp_frame='\n';
    tmp_frame+="ST";
    tmp_frame+="#"+QString::number(stateOut)+";";
    tmp_frame+=QString::number(positionOut.x,'f',3)+";";
    tmp_frame+=QString::number(positionOut.y,'f',3)+";";
    tmp_frame+=QString::number(positionOut.rotate*180.0/M_PI,'f',3)+";";//degree
    tmp_frame+=QString::number(batteryInfoOut.batteryLeftLightmodRatio,'f',2)+";";
    tmp_frame+=QString::number(lineSpeed,'f',3)+";";
    tmp_frame+=QString::number(rotateSpeed,'f',3)+";";
    tmp_frame+=QString::number(isLoaded)+";";
    tmp_frame+=QString::number(errorCode)+";";
    tmp_frame+=QString::number(warnCode)+";";
    tmp_frame+=QString::number(noticeCode)+";";
    tmp_frame+=QString::number(heartBeat)+";";
    tmp_frame+=QString::number(lineDistance,'f',2)+";";
    tmp_frame+=QString::number(angleDistance,'f',2)+";";
    tmp_frame+=QString::number(pathInfoOut.passedStationId)+";";
    tmp_frame+=QString::number(pathInfoOut.currentStationId)+";";
    tmp_frame+=QString::number(pathInfoOut.nextStationId)+";";
    tmp_frame+=QString::number(pathInfoOut.passedPathId)+";";
    tmp_frame+=QString::number(pathInfoOut.currentPathId)+";";
    tmp_frame+=QString::number(pathInfoOut.nextPathId)+";";
    tmp_frame+=QString::fromStdString(mapName)+";";
    tmp_frame+=QString::number(version)+";";
    tmp_frame+=QString::number(tmpDi)+";";
    tmp_frame+=QString::number(tmpDo)+";";
    tmp_frame+=QString::number(liftStatus)+";";
    tmp_frame+=QString::number(rollerStatus)+";";
    tmp_frame+=QString::number(tmpFloorId)+";";
    tmp_frame+=QString::number(agvMode)+";";
    tmp_frame+=QString::number(tmpBorderId)+";";
    tmp_frame+=QString::number(barriermodeOut)+";";
    tmp_frame+=QString::number(diTrigCount);
    tmp_frame+='\r';
//    QByteArray tmpBytes;
    sendData(tmp_frame.toLatin1());
    return 1;

}

int SocketFleetAdapter::sendMessageInfo()
{
    QStringList messageListOut;
    fleetService->getAllMessage(*userInfo,messageListOut);
    QString tmpInfo;
    for(int i=0;i<messageListOut.size();i++)
    {
        tmpInfo+=messageListOut[i]+"; ";
    }
    //组帧
    QString tmp_frame;
    tmp_frame='\n';
    tmp_frame+="MSGINFO";
    tmp_frame+="#"+tmpInfo;
    tmp_frame+='\r';
//    QByteArray tmpBytes;
//    tmp_frame.toLocal8Bit();//受setCodecForLocale影响，会转换为设定的编码。如果本机不支持指定编码，则会按toLatin1处理
   sendData(tmp_frame.toUtf8()) ;// 不受setCodecForLocale影响，强制转换为UTF-8编码
//    sendData(tmp_frame.toLatin1());// 不受setCodecForLocale影响，强制转换为ISO-8859-1编码
   return 1;
}

int SocketFleetAdapter::sendRobotArmInfo()
{
    UpdateDataLv2 updateDataLv2;
    fleetService->getMotionServerInfo(*userInfo,updateDataLv2);


    //组帧
    QString tmp_frame;
    tmp_frame='\n';
    tmp_frame+="ARMINFO";
    tmp_frame+="#"+QString::number(updateDataLv2.globalCurrentState_motion)+";";
    tmp_frame+=QString::number(updateDataLv2.globalDefaultToolNum)+";";
    tmp_frame+=QString::number(updateDataLv2.globalDefaultUserNum)+";";
    if(6==updateDataLv2.currentPositionWorld.positionValue.size())
    {
        for(int i=0;i<6;i++)
        {
            tmp_frame+=QString::number(updateDataLv2.currentPositionWorld.positionValue[i],'f',3)+";";//degree
        }

    }
    else
    {
        for(int i=0;i<6;i++)
        {
            tmp_frame+=QString::number(0)+";";//degree
        }
    }

    tmp_frame+=QString::number(updateDataLv2.currentPositionJoint.positionValue.size())+";";
    for(int i=0;i<updateDataLv2.currentPositionJoint.positionValue.size();i++)
    {
        tmp_frame+=QString::number(updateDataLv2.currentPositionJoint.positionValue[i],'f',3)+";";//degree
    }

    tmp_frame+='\r';
//    QByteArray tmpBytes;
    sendData(tmp_frame.toLatin1());
    return 1;
}

int SocketFleetAdapter::processClientRequest()
{
    //要求调度软件每次只能发送一次请求，等到agv回应后才能继续发下一条命令。如果超时agv没有应答，则清除旧后，重新开始。
    //需要做缓存分析，防止数据帧被从中间分开
    QByteArray tmpCurrentReceive;
    if(1!=receiveData(tmpCurrentReceive) || 0==tmpCurrentReceive.size())
    {
        return 0;
    }
//    tmpCurrentReceive="abcn\ntest\r12what";
    QByteArray validFrame;

    int index=tmpCurrentReceive.lastIndexOf("\r");
//    int index=tmpCurrentReceive.indexOf("\r");//保证只处理一条命令
    if(0<=index)
    {
        validFrame=cashFrame+tmpCurrentReceive.left(index+1);
        cashFrame=tmpCurrentReceive.mid(index+1);

    }
    else
    {
        cashFrame+=tmpCurrentReceive;
        qDebug()<<"调度接收缓存字符串:"<<cashFrame;
        return 0;
    }

    if(cashFrame.size()>10000)
    {
        qDebug()<<"error,,,cashFrame to big"<<cashFrame;
        cashFrame.clear();
    }



    //可能保护多条指令，需要处理
    int validIndex;
    QByteArray oneFrame;
    do
    {
        validIndex=validFrame.indexOf("\r");//保证只处理一条命令
        if(0<=validIndex)
        {
            oneFrame=validFrame.left(validIndex+1);
            validFrame=validFrame.mid(validIndex+1);

        }
        else
        {
            break;
        }
        QString tmp_frame=oneFrame;
        if(1!=executeOneCommandAndReply(tmp_frame))
        {
            break;
        }

    }
    while(0<=validIndex);


    return 1;
}

int SocketFleetAdapter::sendData(const QByteArray &dataIn)
{
    if(dataIn.isEmpty() || NULL==tcpSocket || tcpSocket->state() != QTcpSocket::ConnectedState)
    {
//        qDebug()<<"failed,SocketFleetAdapter::sendData";
        return 0;
    }
    if(!tcpSocket->write(dataIn))
    {
        qDebug() << "SocketFleetAdapter::sendData数据发送失败:" << tcpSocket->errorString();
        return -1;
    }
    tcpSocket->flush();
    return 1;
}

int SocketFleetAdapter::sendOkFrame()
{
    //组帧
    QString tmp_frame="\nOK\r";
    return sendData(tmp_frame.toLatin1());
}

int SocketFleetAdapter::sendAbortFrame()
{
    //组帧
    QString tmp_frame="\nBD\r";
    return sendData(tmp_frame.toLatin1());
}

int SocketFleetAdapter::receiveData(QByteArray &dataOut)
{
    if(NULL==tcpSocket || tcpSocket->state() != QTcpSocket::ConnectedState)
    {
        return 0;
    }
    dataOut = tcpSocket->readAll();
    tcpSocket->flush();
    return 1;
}


void SocketFleetAdapter::addMsg(int messageLevel, string componentName, string messageType,
                           int messageCode, int robotId,
                           QString parameter1, int parameter2, int parameter3, int parameter4)
{
    Message tmpMsg;
    tmpMsg.MessageLevel = messageLevel;
    tmpMsg.componentClassName = componentName;
    tmpMsg.messageType = messageType;
    tmpMsg.robotId = robotId;
    tmpMsg.messageCode = messageCode;


    QString infomationStr;

    switch(messageCode)
    {
    case 12302:
    {
        infomationStr =  QObject::tr("有调度系统连接到AGV，客户ip=")+parameter1;
        break;
    }
    case 12301:
    {
        infomationStr =  QObject::tr("socketFleetConfig.xml配置文件读取失败！")
                .arg(parameter1);
        break;
    }
    case 12300:
    {
        infomationStr =  QObject::tr("套接字调度适配器模块初始化失败！")
                .arg(parameter1);
        break;
    }
    default:

        break;
    }


    tmpMsg.MessageInformation = infomationStr.toStdString();

    MessageLog::getInstance()->addMessage(tmpMsg);
}



